home *** CD-ROM | disk | FTP | other *** search
/ AOL File Library: 2,801 to 2,900 / aol-file-protocol-4400-2801-to-2900.zip / AOLDLs / C++ Files Library / Graphic Gems I, II & III (C_C++) / Graphics Gems C Code.sea / GemsI / Src / Roots3And4.c < prev    next >
Text File  |  1992-06-16  |  5KB  |  242 lines
  1. /*
  2.  *  Roots3And4.c
  3.  *
  4.  *  Utility functions to find cubic and quartic roots,
  5.  *  coefficients are passed like this:
  6.  *
  7.  *      c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
  8.  *
  9.  *  The functions return the number of non-complex roots and
  10.  *  put the values into the s array.
  11.  *
  12.  *  Author:         Jochen Schwarze (schwarze@isa.de)
  13.  *
  14.  *  Jan 26, 1990    Version for Graphics Gems
  15.  *  Oct 11, 1990    Fixed sign problem for negative q's in SolveQuartic
  16.  *                  (reported by Mark Podlipec),
  17.  *                  Old-style function definitions,
  18.  *                  IsZero() as a macro
  19.  *  Nov 23, 1990    Some systems do not declare acos() and cbrt() in
  20.  *                  <math.h>, though the functions exist in the library.
  21.  *                  If large coefficients are used, EQN_EPS should be
  22.  *                  reduced considerably (e.g. to 1E-30), results will be
  23.  *                  correct but multiple roots might be reported more
  24.  *                  than once.
  25.  */
  26.  
  27. #include    <math.h>
  28. extern double   sqrt(), cbrt(), cos(), acos();
  29.  
  30. /* epsilon surrounding for near zero values */
  31.  
  32. #define     EQN_EPS     1e-9
  33. #define        IsZero(x)    ((x) > -EQN_EPS && (x) < EQN_EPS)
  34.  
  35. #ifdef NOCBRT
  36. #define     cbrt(x)     ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
  37.                           ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
  38. #endif
  39.  
  40. int SolveQuadric(c, s)
  41.     double c[ 3 ];
  42.     double s[ 2 ];
  43. {
  44.     double p, q, D;
  45.  
  46.     /* normal form: x^2 + px + q = 0 */
  47.  
  48.     p = c[ 1 ] / (2 * c[ 2 ]);
  49.     q = c[ 0 ] / c[ 2 ];
  50.  
  51.     D = p * p - q;
  52.  
  53.     if (IsZero(D))
  54.     {
  55.     s[ 0 ] = - p;
  56.     return 1;
  57.     }
  58.     else if (D < 0)
  59.     {
  60.     return 0;
  61.     }
  62.     else if (D > 0)
  63.     {
  64.     double sqrt_D = sqrt(D);
  65.  
  66.     s[ 0 ] =   sqrt_D - p;
  67.     s[ 1 ] = - sqrt_D - p;
  68.     return 2;
  69.     }
  70. }
  71.  
  72.  
  73. int SolveCubic(c, s)
  74.     double c[ 4 ];
  75.     double s[ 3 ];
  76. {
  77.     int     i, num;
  78.     double  sub;
  79.     double  A, B, C;
  80.     double  sq_A, p, q;
  81.     double  cb_p, D;
  82.  
  83.     /* normal form: x^3 + Ax^2 + Bx + C = 0 */
  84.  
  85.     A = c[ 2 ] / c[ 3 ];
  86.     B = c[ 1 ] / c[ 3 ];
  87.     C = c[ 0 ] / c[ 3 ];
  88.  
  89.     /*  substitute x = y - A/3 to eliminate quadric term:
  90.     x^3 +px + q = 0 */
  91.  
  92.     sq_A = A * A;
  93.     p = 1.0/3 * (- 1.0/3 * sq_A + B);
  94.     q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
  95.  
  96.     /* use Cardano's formula */
  97.  
  98.     cb_p = p * p * p;
  99.     D = q * q + cb_p;
  100.  
  101.     if (IsZero(D))
  102.     {
  103.     if (IsZero(q)) /* one triple solution */
  104.     {
  105.         s[ 0 ] = 0;
  106.         num = 1;
  107.     }
  108.     else /* one single and one double solution */
  109.     {
  110.         double u = cbrt(-q);
  111.         s[ 0 ] = 2 * u;
  112.         s[ 1 ] = - u;
  113.         num = 2;
  114.     }
  115.     }
  116.     else if (D < 0) /* Casus irreducibilis: three real solutions */
  117.     {
  118.     double phi = 1.0/3 * acos(-q / sqrt(-cb_p));
  119.     double t = 2 * sqrt(-p);
  120.  
  121.     s[ 0 ] =   t * cos(phi);
  122.     s[ 1 ] = - t * cos(phi + M_PI / 3);
  123.     s[ 2 ] = - t * cos(phi - M_PI / 3);
  124.     num = 3;
  125.     }
  126.     else /* one real solution */
  127.     {
  128.     double sqrt_D = sqrt(D);
  129.     double u = cbrt(sqrt_D - q);
  130.     double v = - cbrt(sqrt_D + q);
  131.  
  132.     s[ 0 ] = u + v;
  133.     num = 1;
  134.     }
  135.  
  136.     /* resubstitute */
  137.  
  138.     sub = 1.0/3 * A;
  139.  
  140.     for (i = 0; i < num; ++i)
  141.     s[ i ] -= sub;
  142.  
  143.     return num;
  144. }
  145.  
  146.  
  147. int SolveQuartic(c, s)
  148.     double c[ 5 ]; 
  149.     double s[ 4 ];
  150. {
  151.     double  coeffs[ 4 ];
  152.     double  z, u, v, sub;
  153.     double  A, B, C, D;
  154.     double  sq_A, p, q, r;
  155.     int     i, num;
  156.  
  157.     /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
  158.  
  159.     A = c[ 3 ] / c[ 4 ];
  160.     B = c[ 2 ] / c[ 4 ];
  161.     C = c[ 1 ] / c[ 4 ];
  162.     D = c[ 0 ] / c[ 4 ];
  163.  
  164.     /*  substitute x = y - A/4 to eliminate cubic term:
  165.     x^4 + px^2 + qx + r = 0 */
  166.  
  167.     sq_A = A * A;
  168.     p = - 3.0/8 * sq_A + B;
  169.     q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
  170.     r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
  171.  
  172.     if (IsZero(r))
  173.     {
  174.     /* no absolute term: y(y^3 + py + q) = 0 */
  175.  
  176.     coeffs[ 0 ] = q;
  177.     coeffs[ 1 ] = p;
  178.     coeffs[ 2 ] = 0;
  179.     coeffs[ 3 ] = 1;
  180.  
  181.     num = SolveCubic(coeffs, s);
  182.  
  183.     s[ num++ ] = 0;
  184.     }
  185.     else
  186.     {
  187.     /* solve the resolvent cubic ... */
  188.  
  189.     coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
  190.     coeffs[ 1 ] = - r;
  191.     coeffs[ 2 ] = - 1.0/2 * p;
  192.     coeffs[ 3 ] = 1;
  193.  
  194.     (void) SolveCubic(coeffs, s);
  195.  
  196.     /* ... and take the one real solution ... */
  197.  
  198.     z = s[ 0 ];
  199.  
  200.     /* ... to build two quadric equations */
  201.  
  202.     u = z * z - r;
  203.     v = 2 * z - p;
  204.  
  205.     if (IsZero(u))
  206.         u = 0;
  207.     else if (u > 0)
  208.         u = sqrt(u);
  209.     else
  210.         return 0;
  211.  
  212.     if (IsZero(v))
  213.         v = 0;
  214.     else if (v > 0)
  215.         v = sqrt(v);
  216.     else
  217.         return 0;
  218.  
  219.     coeffs[ 0 ] = z - u;
  220.     coeffs[ 1 ] = q < 0 ? -v : v;
  221.     coeffs[ 2 ] = 1;
  222.  
  223.     num = SolveQuadric(coeffs, s);
  224.  
  225.     coeffs[ 0 ]= z + u;
  226.     coeffs[ 1 ] = q < 0 ? v : -v;
  227.     coeffs[ 2 ] = 1;
  228.  
  229.     num += SolveQuadric(coeffs, s + num);
  230.     }
  231.  
  232.     /* resubstitute */
  233.  
  234.     sub = 1.0/4 * A;
  235.  
  236.     for (i = 0; i < num; ++i)
  237.     s[ i ] -= sub;
  238.  
  239.     return num;
  240. }
  241.  
  242.